Assignment 9: Development of a tracking filter of a moving object when measurements and motion models are in different coordinate systems.

Team №6:

  1. Angelina Prokopeva
  2. Nikita Gorbadey
  3. Mark Griguletskii
  4. Stanislav Savushkin

03.10.2019, Skoltech

In [1]:
import numpy as np
from numpy import linalg as LA
import pandas as pd
import matplotlib.pyplot as plt
import plotly.graph_objects as go
from IPython.display import Image
import os

if not os.path.exists("images"):
    os.mkdir("images")
In [2]:
generate_report = False
def plot(trace_num, x_data, y_data, xlable = 'xlable', ylable = 'ylable',
                legend = 'legend', title = 'title', mode='lines'):
    plot.counter += 1
    fig_name = 'images/' + str(plot.counter) + '.jpg'
    fig = go.Figure()
    for i in range(trace_num):
        fig.add_trace(go.Scatter(x=x_data[i], y=y_data[i], mode=mode, name=legend[i], showlegend = True))
    fig.update_layout(
        title=go.layout.Title(
            text=title,
        ),
        xaxis=go.layout.XAxis(
            title=go.layout.xaxis.Title(
                text=xlable
            )
        ),
        yaxis=go.layout.YAxis(
            title=go.layout.yaxis.Title(
                text=ylable
            )
        )
    )
    if generate_report is True:
        fig.write_image(fig_name)
        display(Image(fig_name))
    else:
        fig.show()
plot.counter = 0
In [3]:
def plot_polar(trace_num, r, theta, xlable = 'xlable', ylable = 'ylable',
                legend = 'legend', title = 'title', mode='lines', size=500):
    fig = go.Figure()
    r_min = np.min(r[0])
    r_max = np.max(r[0])
    for i in range(trace_num):
        fig.add_trace(go.Scatterpolar(r = r[i], theta = theta[i], mode=mode[i],
                                      name=legend[i], showlegend = True))
        if r_min > np.min(r[i]):
            r_min = np.min(r[i])
        if r_max < np.max(r[i]):
            r_max = np.max(r[i])
    fig.update_layout(
        width=size,
        height=size,
        title=go.layout.Title(
                text=title,
            ),
        polar = dict(
            radialaxis = dict(range=[r_min*0.99, r_max*1.01])
            )
        )
    fig.show()

%D0%A1%D0%BD%D0%B8%D0%BC%D0%BE%D0%BA%20%D1%8D%D0%BA%D1%80%D0%B0%D0%BD%D0%B0%202019-10-16%20%D0%B2%2022.01.04.png

In [4]:
def generate_trajectory(n=26, T=2, Vx_init=-50, Vy_init=-45, x0=13500/np.sqrt(2), y0=13500/np.sqrt(2)):
    x = np.zeros(n)
    y = np.zeros(n)
    Vx = np.full(n, Vx_init)
    Vy = np.full(n, Vy_init)
    x[0] = x0
    y[0] = y0
    x[1:] = Vx[:-1]*T
    y[1:] = Vy[:-1]*T
    return [np.cumsum(x), Vx, np.cumsum(y), Vy]
In [5]:
N = 26
x, Vx, y, Vy = generate_trajectory()
steps = np.linspace(1, N, num=N)
In [6]:
plot(1, [x], [y], legend=['True trajectory'], title='True trajectory in Cartesian coordinates',
     xlable='X', ylable='Y')

%D0%A1%D0%BD%D0%B8%D0%BC%D0%BE%D0%BA%20%D1%8D%D0%BA%D1%80%D0%B0%D0%BD%D0%B0%202019-10-17%20%D0%B2%201.50.45.png

In [7]:
def cortes2polar(x, y):
    return [np.sqrt(x[:]**2 + y[:]**2), np.arctan2(x[:], y[:])]
In [8]:
D, beta = cortes2polar(x, y)
plot_polar(1, [D], [np.degrees(beta)], legend=['true trajectory'], mode=['lines'],
           title = 'True trajectory in polar coordinates')

%D0%A1%D0%BD%D0%B8%D0%BC%D0%BE%D0%BA%20%D1%8D%D0%BA%D1%80%D0%B0%D0%BD%D0%B0%202019-10-17%20%D0%B2%202.17.03.png

In [9]:
def measure_polar_coord(D, beta, sigma_D=20, sigma_b=0.02):
    return [D+np.random.normal(loc=0, scale=sigma_D, size=D.size),
            beta+np.random.normal(loc=0, scale=sigma_b, size=beta.size)]
In [10]:
D_mes, beta_mes = measure_polar_coord(D, beta)
plot_polar(2, [D, D_mes], [np.degrees(beta), np.degrees(beta_mes)], title='Object movement in polar coordinates',
           legend=['true tr', 'measured tr'], mode=['lines', 'lines'], size=700)

%D0%A1%D0%BD%D0%B8%D0%BC%D0%BE%D0%BA%20%D1%8D%D0%BA%D1%80%D0%B0%D0%BD%D0%B0%202019-10-17%20%D0%B2%202.21.32.png

%D0%A1%D0%BD%D0%B8%D0%BC%D0%BE%D0%BA%20%D1%8D%D0%BA%D1%80%D0%B0%D0%BD%D0%B0%202019-10-17%20%D0%B2%202.22.21.png

In [11]:
def polar2cartes(D, beta):
    return D[:]*np.sin(beta[:]), D[:]*np.cos(beta[:])

%D0%A1%D0%BD%D0%B8%D0%BC%D0%BE%D0%BA%20%D1%8D%D0%BA%D1%80%D0%B0%D0%BD%D0%B0%202019-10-17%20%D0%B2%202.31.05.png

In [12]:
z = np.column_stack(polar2cartes(D_mes, beta_mes))
plot(2, [x, z[:, 0]], [y, z[:, 1]], legend=['true tr', 'pseudo mes tr'],
     title='Trajectrory', xlable='X', ylable='Y')

%D0%A1%D0%BD%D0%B8%D0%BC%D0%BE%D0%BA%20%D1%8D%D0%BA%D1%80%D0%B0%D0%BD%D0%B0%202019-10-17%20%D0%B2%202.35.21.png

%D0%A1%D0%BD%D0%B8%D0%BC%D0%BE%D0%BA%20%D1%8D%D0%BA%D1%80%D0%B0%D0%BD%D0%B0%202019-10-17%20%D0%B2%202.36.09.png

%D0%A1%D0%BD%D0%B8%D0%BC%D0%BE%D0%BA%20%D1%8D%D0%BA%D1%80%D0%B0%D0%BD%D0%B0%202019-10-17%20%D0%B2%202.36.35.png

Supplementary function for calculating covariance matrix R

In [13]:
def calculate_R(D_mes, beta_mes, sigma_D = 20, sigma_b = 0.02):
    r11 = (np.sin(beta_mes[:]) * sigma_D)**2 + ((D_mes[:])**2)*(np.cos(beta_mes[:])*sigma_b)**2
    r12 = np.sin(beta_mes[:])*np.cos(beta_mes[:])*(sigma_D**2 - ((D_mes[:])**2)*sigma_b**2)
    r21 = np.sin(beta_mes[:])*np.cos(beta_mes[:])*(sigma_D**2 - ((D_mes[:])**2)*sigma_b**2)
    r22 = (np.cos(beta_mes[:]) * sigma_D)**2 + ((D_mes[:])**2)*(np.sin(beta_mes[:])*sigma_b)**2
    return np.column_stack((r11,r12, r21, r22)).reshape((26, 2, 2))
In [14]:
R = calculate_R(D_mes, beta_mes)
In [15]:
def kalman_polar(z, R, T=2, X_0=[40000, -20, 40000, -20], N=26,
                 P_0=np.eye(4)*10**10, sigma_D=2, sigma_b=0.02, shift=7):
    shape = (N, 4)
    X_pred = np.zeros(shape)
    X_filt = np.zeros(shape)
    D_pred = np.zeros(shape[0])
    D_filt = np.zeros(shape[0])
    beta_pred = np.zeros(shape[0])
    beta_filt = np.zeros(shape[0])
    F = np.array([[1, T, 0, 0], 
                  [0, 1, 0, 0], 
                  [0, 0, 1, T], 
                  [0, 0, 0, 1]])
    H = np.array([[1, 0, 0, 0],
                 [0, 0, 1, 0]])

    
    # prepare parameters
    P_pred = np.zeros((shape[0], 4, 4))
    P_filt = np.zeros((shape[0], 4, 4))
    K = np.zeros((shape[0], 4, 2))

    X_filt[0] = X_0
    P_filt[0] = P_0
    D_filt[0] = np.sqrt(X_filt[0][0]**2 + X_filt[0][2]**2)
    beta_filt[0] = np.arctan2(X_filt[0][0], X_filt[0][2]) 

    for i in range(1, shape[0]):
        # Make prediction
        X_pred[i] = F.dot(X_filt[i-1])
        P_pred[i] = np.dot(F, np.dot(P_filt[i-1], F.T))
        D_pred[i] = np.sqrt(X_pred[i][0]**2 + X_pred[i][2]**2)
        beta_pred[i] = np.arctan2(X_pred[i][0], X_pred[i][2]) 

        # Make filtration
        K[i] = P_pred[i].dot(H.T).dot(np.linalg.inv(H.dot(P_pred[i].dot(H.T)) + R[i]))
        X_filt[i] = X_pred[i] + K[i].dot(z[i] - H.dot(X_pred[i]))
        P_filt[i] = (np.eye(shape[1]) - np.dot(K[i], H)).dot(P_pred[i])
        D_filt[i] = np.sqrt(X_filt[i][0]**2 + X_filt[i][2]**2)
        beta_filt[i] = np.arctan2(X_filt[i][0], X_filt[i][2]) 

    return D_filt, beta_filt, D_pred, beta_pred, K, X_filt, X_pred
In [16]:
D_filt, beta_filt, D_pred, beta_pred, K, X_filt, X_pred = kalman_polar(z, R)
plot_polar(3, [D[1:], D_mes[1:], D_filt[1:]],
           [np.degrees(beta[1:]), np.degrees(beta_mes[1:]), np.degrees(beta_filt[1:])],
           legend=['true tr', 'measured tr', 'kalman filter'], size = 1000,
           mode = ['lines', 'markers', 'lines'], title='Kalman filter visualization')

From the plot we can see that Kalman filter obtained trajectory starts from measurements points and then moves closer to true trajectory.

%D0%A1%D0%BD%D0%B8%D0%BC%D0%BE%D0%BA%20%D1%8D%D0%BA%D1%80%D0%B0%D0%BD%D0%B0%202019-10-17%20%D0%B2%203.54.57.png

In [17]:
def error_calculation(runs=500, n=26, x0=13500/np.sqrt(2), y0=13500/np.sqrt(2), sigma_D=20, sigma_b=0.02):
    true_err_pred_D = np.zeros(n)
    true_err_pred_beta = np.zeros(n)
    true_err_filt_D = np.zeros(n)
    true_err_filt_beta = np.zeros(n)
    
    for i in range(runs):
        x, Vx, y, Vy = generate_trajectory(x0=x0, y0=y0)
        D, beta = cortes2polar(x, y)
        D_mes, beta_mes = measure_polar_coord(D, beta, sigma_D=sigma_D, sigma_b=sigma_b)
        z = np.column_stack(polar2cartes(D_mes, beta_mes))
        R = calculate_R(D_mes, beta_mes)
        D_filt, beta_filt, D_pred, beta_pred, K, _, _ = kalman_polar(z, R)
        
        true_err_pred_D += (D_pred - D)**2
        true_err_pred_beta += (beta_pred - beta)**2
        true_err_filt_D += (D_filt - D)**2
        true_err_filt_beta += (beta_filt - beta)**2
        
    true_err_pred_D = np.sqrt(true_err_pred_D/(runs-1))
    true_err_pred_beta = np.sqrt(true_err_pred_beta/(runs-1))
    true_err_filt_D = np.sqrt(true_err_filt_D/(runs-1))
    true_err_filt_beta = np.sqrt(true_err_filt_beta/(runs-1))
    
    return true_err_pred_D, true_err_pred_beta, true_err_filt_D, true_err_filt_beta
In [18]:
M = 500
N = 26
true_err_pred_D, true_err_pred_beta, true_err_filt_D, true_err_filt_beta = error_calculation(runs=M, n=N)
steps = np.linspace(1, N, num=N)
In [19]:
plot(3, [steps[3:], steps[3:], steps[3:]], [true_err_pred_D[3:], true_err_filt_D[3:], np.full(N, 20)[3:]],
     legend=['prediction error', 'filtration error', 'range variance'],
     title='Error evolution', xlable='step', ylable='value')

From the plot we can see that both filtration and prediction errors decrease with time and ultimately are under range variance threshold

In [20]:
plot(3, [steps[3:], steps[3:], steps[3:]], [true_err_pred_beta[3:], true_err_filt_beta[3:], np.full(N, 0.02)[3:]],
     legend=['prediction error', 'filtration error', 'range variance'],
     title='Error evolution', xlable='step', ylable='value')

The same we can say for azimuth error calculation

%D0%A1%D0%BD%D0%B8%D0%BC%D0%BE%D0%BA%20%D1%8D%D0%BA%D1%80%D0%B0%D0%BD%D0%B0%202019-10-17%20%D0%B2%204.29.48.png

In [21]:
x, Vx, y, Vy = generate_trajectory()
D, beta = cortes2polar(x, y)
D_mes, beta_mes = measure_polar_coord(D, beta)
z = np.column_stack(polar2cartes(D_mes, beta_mes))
R = calculate_R(D_mes, beta_mes)
D_filt, beta_filt, D_pred, beta_pred, K, X_filt, X_pred = kalman_polar(z, R)
plot(1, [beta[:]], [x[:]], title='Dependence of coordinate x on azimuth beta',
     xlable='beta', ylable='x', legend=['dependence x on beta'])

Here we can see that under this conditions arctg is close to line, so we expect linearization errors insignificant

%D0%A1%D0%BD%D0%B8%D0%BC%D0%BE%D0%BA%20%D1%8D%D0%BA%D1%80%D0%B0%D0%BD%D0%B0%202019-10-17%20%D0%B2%204.46.01.png

In [22]:
def cond_num(R):
    eigs, v = LA.eig(R)
    out = np.zeros(len(eigs))
    eig1 = eigs[:, 0]
    eig2 = eigs[:, 1]
    for i in range(len(eigs)):
        out[i] = eig1[i]/eig2[i] if eig1[i] > eig2[i] else eig2[i]/eig1[i]
    return out
In [23]:
cond = cond_num(R)
plot(1, [steps], [cond], title='Condition number evolution', xlable='step', ylable='value', legend=['cond num'])

From the plot we can see that condition number decreases with time. Also it is within range (100, 190) so matrix 𝑅 is well-conditioned.

%D0%A1%D0%BD%D0%B8%D0%BC%D0%BE%D0%BA%20%D1%8D%D0%BA%D1%80%D0%B0%D0%BD%D0%B0%202019-10-17%20%D0%B2%204.54.25.png

In [24]:
plot(1, [steps], [K[:, 0, 0]], title='Filter gain evoluton', xlable='step', ylable='value',
     legend=['filter gain'])

As long as we work in different coordinate systems, filter gain is not representive in this case

%D0%A1%D0%BD%D0%B8%D0%BC%D0%BE%D0%BA%20%D1%8D%D0%BA%D1%80%D0%B0%D0%BD%D0%B0%202019-10-17%20%D0%B2%204.58.11.png

In [25]:
M = 500
N = 26
x0=3500/np.sqrt(2)
y0=3500/np.sqrt(2)
x, Vx, y, Vy = generate_trajectory(x0=x0, y0=y0)
D, beta = cortes2polar(x, y)
D_mes, beta_mes = measure_polar_coord(D, beta)
z = np.column_stack(polar2cartes(D_mes, beta_mes))
R = calculate_R(D_mes, beta_mes)
D_filt, beta_filt, D_pred, beta_pred, K, X_filt, X_pred = kalman_polar(z, R)
plot_polar(3, [D[1:], D_mes[1:], D_filt[1:]],
           [np.degrees(beta[1:]), np.degrees(beta_mes[1:]), np.degrees(beta_filt[1:])],
           title='Kalman filter visualization',
           legend=['true tr', 'measured tr', 'kalman filter'], size = 1000,
           mode = ['lines', 'markers', 'lines'])

On the plot we can see that visually Kalman filter still works good

%D0%A1%D0%BD%D0%B8%D0%BC%D0%BE%D0%BA%20%D1%8D%D0%BA%D1%80%D0%B0%D0%BD%D0%B0%202019-10-17%20%D0%B2%205.12.01.png

In [26]:
true_err_pred_D, true_err_pred_beta, true_err_filt_D, true_err_filt_beta = error_calculation(runs=M, n=N,
                                                                                             x0=x0, y0=y0)
steps = np.linspace(1, N, num=N)
In [27]:
plot(3, [steps[3:], steps[3:], steps[3:]], [true_err_pred_D[3:], true_err_filt_D[3:], np.full(N, 20)[3:]],
     legend=['prediction error', 'filtration error', 'range variance'],
     title='Error evolution', xlable='step', ylable='value')

On the plot we can see that range error decreases with time and smaller than measurements variance

In [28]:
plot(3, [steps[3:], steps[3:], steps[3:]], [true_err_pred_beta[3:], true_err_filt_beta[3:], np.full(N, 0.02)[3:]],
     legend=['prediction error', 'filtration error', 'range variance'],
     title='Error evolution', xlable='step', ylable='value')

From the plot we can see that at the beginning azimut error decreases so that filter works optimally, but at the end when the distance to the object become small, the error increases.

%D0%A1%D0%BD%D0%B8%D0%BC%D0%BE%D0%BA%20%D1%8D%D0%BA%D1%80%D0%B0%D0%BD%D0%B0%202019-10-17%20%D0%B2%205.11.39.png

In [29]:
plot(1, [beta], [x], title='Dependence of coordinate x on azimuth beta',
     xlable='beta', ylable='x', legend=['dependence x on beta'])

From the plot we can see that when the object is close, the arctan function can't be considered as line so we expect big linearisation error

%D0%A1%D0%BD%D0%B8%D0%BC%D0%BE%D0%BA%20%D1%8D%D0%BA%D1%80%D0%B0%D0%BD%D0%B0%202019-10-17%20%D0%B2%205.14.31.png

In [30]:
cond = cond_num(R)
plot(2, [steps, steps], [cond, np.full(N, 1)], title='Condition number evolution', xlable='step', ylable='value',
     legend=['cond num', 'referal val'])

From the plot we can see that condition number firstly decreases but when object come closer it starts to increase

%D0%A1%D0%BD%D0%B8%D0%BC%D0%BE%D0%BA%20%D1%8D%D0%BA%D1%80%D0%B0%D0%BD%D0%B0%202019-10-17%20%D0%B2%205.16.18.png

Linearization errors bigger when object starts movement closer to observer, so we can see bigger error in this case.

%D0%A1%D0%BD%D0%B8%D0%BC%D0%BE%D0%BA%20%D1%8D%D0%BA%D1%80%D0%B0%D0%BD%D0%B0%202019-10-17%20%D0%B2%205.16.46.png

%D0%A1%D0%BD%D0%B8%D0%BC%D0%BE%D0%BA%20%D1%8D%D0%BA%D1%80%D0%B0%D0%BD%D0%B0%202019-10-17%20%D0%B2%205.17.30.png

In [31]:
M = 500
N = 26
x0=3500/np.sqrt(2)
y0=3500/np.sqrt(2)
sigma_D=50
sigma_b=0.0015

x, Vx, y, Vy = generate_trajectory(x0=x0, y0=y0)
D, beta = cortes2polar(x, y)
D_mes, beta_mes = measure_polar_coord(D, beta, sigma_D=sigma_D, sigma_b=sigma_b)
z = np.column_stack(polar2cartes(D_mes, beta_mes))
R = calculate_R(D_mes, beta_mes, sigma_D=sigma_D, sigma_b=sigma_b)
D_filt, beta_filt, D_pred, beta_pred, K, X_filt, X_pred = kalman_polar(z, R)
plot_polar(3, [D[1:], D_mes[1:], D_filt[1:]],
           [np.degrees(beta[1:]), np.degrees(beta_mes[1:]), np.degrees(beta_filt[1:])],
           legend=['true tr', 'measured tr', 'kalman filter'], size = 1000,
           mode = ['lines', 'markers', 'lines'], title='Kalman filter visualization')
In [32]:
true_err_pred_D, true_err_pred_beta, true_err_filt_D, true_err_filt_beta = error_calculation(runs=M, n=N, 
                                                                                             x0=x0, y0=y0,
                                                                                             sigma_D=sigma_D,
                                                                                             sigma_b=sigma_b)
steps = np.linspace(1, N, num=N)
In [33]:
plot(3, [steps[3:], steps[3:], steps[3:]], [true_err_pred_D[3:], true_err_filt_D[3:], np.full(N, sigma_D)[3:]],
     legend=['prediction error', 'filtration error', 'range variance'], title='Error evolution')

From the plot we can see that range error is still good for this case

In [34]:
plot(3, [steps[3:], steps[3:], steps[3:]], [true_err_pred_beta[3:], true_err_filt_beta[3:], np.full(N, sigma_b)[3:]],
     legend=['prediction error', 'filtration error', 'range variance'], title='Error evolution')

From the plot we can see that azimut error is about the same as measurement variance at first, but when object moves closer to observer, the error increases significantly

In [35]:
cond = cond_num(R)
plot(2, [steps, steps], [cond, np.full(N, 1)], title='Condition number evolution', xlable='step', ylable='value',
     legend=['cond num', 'referal val'])

Here we can see that when object comes closer to observer, the condition number increases drammaticly and we switch to ill-state

Conclusion

We have learned how to develop a tracking filter of a moving object when measurements and motion model are in different coordinate systems, it is typical problem in practical application, for example radiolocation. We tryed to use Kalman filter for different initial conditions: different distance from an observer and different measurement noises. We analyzed instability zone of a tracking filter due to ill-conditioned coordinate transformations of measurements.

We got that filter may diverge (it means conditions navigation system may become blind) when the system enters ill-condition, when the object is too close to observer. Also when object is close to observer, we have bigger linearization errors. The linearization error is higher that filtration error beacause we have problem only with azimuth